#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <arch/philips/lpc2103.h>
 
#include "sys.h"
#include "fixed.h"
#include "qfilt.h"
#include "qfir.h"
#include "util.h"
#include "car.h"

//#define SLOW_DOWN_FOR_TURNS 1
#define USE_3_SENSORS 1
static const fixedp Kp = float2q(120.0);		// GAIN
static const fixedp Kd = float2q(0.0);
static const fixedp dt = float2q(0.00025);

#define CALIBRATE_SAMPLES 1024
#define NUM_SENSORS 3

#define LEFT -1
#define RIGHT 1

#define SLEFT 0			// indexes into the sensors array
#define SRIGHT 1
#define SCENTER 2
#define SCENTER_FWD 3

const int SENSOR_CHN[] = {0, 1, 2, 3};

static void sample_ISR() __attribute__ ((interrupt ("IRQ")));
static void adc_ISR() __attribute__ ((interrupt ("IRQ")));
static void calibrate_ISR() __attribute__ ((interrupt ("IRQ")));


#define H1_N 20
#define H1_FRACBITS 28
static const fixedp H1_H[] = {
0xffe95b13, 0xffd83617, 0xffd0ea4d, 0xffe16d90, 0x0015c188, 0x00728ea4, 0x00f1429e,
0x017f8eec, 0x02030459, 0x025ff997, 0x02818c76, 0x025ff997, 0x02030459, 0x017f8eec,
0x00f1429e, 0x00728ea4, 0x0015c188, 0xffe16d90, 0xffd0ea4d, 0xffd83617, 0xffe95b13};


/*
#define H1_N 4
#define H1_FRACBITS 20
static const fixedp H1_H[] = {0x00040000, 0x00040000, 0x00040000, 0x00040000};
*/

/*
#define H1_N 10
#define H1_FRACBITS 28
static const fixedp H1_H[] = {
0xffef11e3, 0x00075cb0, 0x00abe50f, 0x01fc0f74, 0x036b99c7, 0x040d9f50, 0x036b99c7, 0x01fc0f74, 0x00abe50f, 0x00075cb0, 0xffef11e3};
*/

/*
#define H1_N 30
#define H1_FRACBITS 28
static const fixedp H1_H[] = {
0xfff9d204, 0xfff27e84, 0xffe99323, 0xffe153d2, 0xffdd3116, 0xffe157ea, 0xfff1ef0f, 0x0012221f, 0x00432e42, 0x0083ac7a, 0x00cf4eaf, 0x011f2f3b, 0x016ab2c7, 0x01a8d9ab, 0x01d1c147, 0x01e006f4, 0x01d1c147, 0x01a8d9ab, 0x016ab2c7, 0x011f2f3b, 0x00cf4eaf, 0x0083ac7a, 0x00432e42, 0x0012221f, 0xfff1ef0f, 0xffe157ea, 0xffdd3116, 0xffe153d2, 0xffe99323, 0xfff27e84, 0xfff9d204};
*/

#define H2_L 1
#define H2_SOS_FRACBITS 28
#define H2_G_FRACBITS 28
static const fixedp H2_SOS[] = {0x20000000, 0xe38ab968, 0x0cd01670};
static const fixedp H2_G[] = {0x0016b3f6};

#define H3_L 1
#define H3_SOS_FRACBITS 29
#define H3_G_FRACBITS 32
static const fixedp H3_SOS[] = {0x40000000, 0xc01232b3, 0x1fedd278};
static const fixedp H3_G[] = {0x00000a56};

/*
#define H4_N 4
#define H4_FRACBITS 20
static const fixedp H4_H[] = {0x00040000, 0x00040000, 0x00040000, 0x00040000};
*/

#define H4_N 2
#define H4_FRACBITS 20
static const fixedp H4_H[] = {0x00080000, 0x00080000};

typedef struct {
	qfir h1;								// lowpass filter for the incoming data
	fixedp h1_w[H1_N];						// storage for the filter
	fixedp scale;							// scale factor for the channel determined by calibration
	fixedp y;								// contains the current filtered output for this sensor
	fixedp centered;						// contains the stabilized value for this channel
	fixedp offTrackThresh;					// a value slightly less than centered, used for
											// determining saturation mode
	short chn;								// which ADC channel is this sensor on
} sensor_t;

sensor_t sensors[NUM_SENSORS];				// array of sensor structures

volatile bool quit = false;					// should we break out of the control loop?
volatile bool samplesReady = false;			// is a new set of samples ready?
volatile bool samplesProcessed = false;		// has the current set of samples been processed?
volatile int sideOfTrack = 0;				// which side of the track are we on? (LEFT or RIGHT)
volatile short adc_buf[8];					// holds the results of ADC conversions
qfilt frontFilt;							// filter for filtering the front center sensor
fixedp frontFilt_w[2 * H3_L];				// storage for frontFilt
fixedp frontFiltered = 0;					// output of frontFilt
qfir errorFilt;								// filter for error signal
fixedp errorFilt_w[H4_N];					// storage for errorFilt

fixedp errorOffsetL;						// error offsets
fixedp errorOffsetR;	

void sensor_init(sensor_t *s, int chn)
{
	qfir_init(&s->h1, H1_H, H1_FRACBITS, s->h1_w, H1_N);
	s->scale = int2q(1);
	s->y = 0;
	s->chn = chn;
}

void init_sensors()
{
	// initialize all the entries in the sensors data structure
	for(int i = 0; i < NUM_SENSORS; i++) {
		sensor_init(&sensors[i], SENSOR_CHN[i]);
	}
}

// Initializes the ADC to sample on channels 0, 1, 2, 5 at 4khz
// No interrupt is generated, so the AD status registers must be polled
// Analog signals are connected to Analog pins 0, 1, 2, and 5 of the ARMMite pro.
// These correspond to AD0.0, AD0.1, AD0.2, AD0.5 of the LPC2103
void adc_init(void)
{
	// want to configure timer2 so that MAT2.0 generates an interrupt at 4khz (our sampling rate)
	T2TCR = 2;						// put timer2 in reset
	T2CTCR = 0;						// select timer mode
	T2PR = 0;						// select a prescale of 15 so that we have a resolution of 1us
	T2MCR = (1 << 0) | (1 << 1);	// reset and interrupt on MAT2.0
	T2MR0 = 3750;					// use 1250 for 12Khz, 3750 for 4Khz, 7500 for 2Khz, 3000 for 5Khz
	
	VICVectAddr0 = (unsigned long)sample_ISR;
	VICVectCntl0 = 26 | (1 << 5);
	VICIntEnable |= (1 << 26);			// enable interrupt for timer2 [man47]
	
	PINSEL0 |= (3 << 20) | (3 << 22) | (3 << 24);		// configure pins AD0.3, AD0.4, AD0.5
	PINSEL1 |= (3 << 12) | (3 << 14) | (3 << 16);		// configure pins AD0.0, AD0.1, AD0.2
	
	VICIntEnClr |= (1 << 18);			// set up ISR for the ADC
	VICVectAddr1 = (unsigned long)adc_ISR;
	VICVectCntl1 = 18 | (1 << 5);		// interrupt source with index 18 (ad0) is enabled
										// as the one with priority 1. man[51], man[58]

	
	
	unsigned int mask = 0;
	for(int i = 0; i < NUM_SENSORS; i++) {
		mask |= 1 << sensors[i].chn;		// select ADC channels
	}
	ADINTEN = 1 << 8;						// enable interrupts on conversion completion
	ADCR = mask | (3 << 8) | (1 << 21);		// CLKDIV = 3, 10 bit, operational
	
	samplesReady = false;
	samplesProcessed = true;
	
	T2TCR = 1;						// take timer1 out of reset. i.e., start sampling
}

void adc_stop(void)
{
	VICIntEnClr |= 1 << 26;			// disable interrupt for timer2 [man47]
	VICVectAddr0 = 0;
	VICVectCntl0 = 0;
	T2TCR = 0;
	
	VICIntEnClr |= 1 << 18;			// disable interrupts for the ADC
	VICVectAddr1 = 0;
	VICVectCntl1 = 0;
	ADCR = 0;
}

void sample_ISR(void)
{
	ADCR |= 1 << 16; 				// start a burst conversion
	samplesReady = false;
	VICIntEnable |= 1 << 18;		// enable ADC interrupts
	T2IR = 0xff;					// clear all interrupts by writing ones
	VICVectAddr = 0;
}

void adc_ISR()
{
	int chn = (ADGDR >> 24) & 7;				// get the channel
	adc_buf[chn] = (ADGDR >> 6) & 0x3ff;		// get the 10-bit value and store it
	if(chn == SENSOR_CHN[NUM_SENSORS - 1]) {	// is this the last channel?
		// if the last channel is done, that means we can stop conversions
		VICIntEnClr |= 1 << 18;					// disable this ISR
		ADCR &= ~(1 << 16);						// clear BURST bit
		samplesReady = true;
		if(!samplesProcessed) {
			// if we get here, we're in trouble. This means that a new sample
			// is ready before the previous one is finished being processed.
			// i.e, our code is too slow
			T2MCR = 0;			// disable timer interrupts
			ADINTEN = 0;		// disable ADC interrupts
			uart0_puts("ERROR: A new sample is ready before the previous one is done being processed.\n");
			quit = true;
		}
		samplesProcessed = false;
	}
	VICVectAddr = 0;
}


static short *calibrateBuf = 0;				// pointer to buffer that calibrate_ISR can use
static volatile int calibrateDone = 0;		// signal from calibrate_ISR to sensor_calibrate that
											// samples have finished being collected
static int calibrateIndex = 0;				// the index into the samples buffer

// gets the offset and rms value for a sensor
void sensor_calibrate(const sensor_t *s, short *_offset)
{
	short buf[CALIBRATE_SAMPLES];
	
	calibrateBuf = buf;
	calibrateDone = 0;
	calibrateIndex = 0;	
	
	VICIntEnClr |= 1 << 18;				// disable interrupts for ad0
	
	// enable interrupts on conversion completion for the specified channel
	// we're sampling at maximum speed, which is 389.610Khz
	VICVectAddr5 = (unsigned long)calibrate_ISR;
	VICVectCntl5 = 18 | (1 << 5);		// interrupt source with index 18 (ad0) is enabled
										// as the one with priority 5. man[51], man[58]
	// CLKS = 3, 10 bit, burst, operational
	ADINTEN = 1 << 8;
	ADCR = (1 << s->chn) | (3 << 8) | (1 << 16) | (1 << 21);
	VICIntEnable |= 1 << 18;			// enable interrupt for ad0 [man47]
	
	// wait for samples to be collected
	while(!calibrateDone);
	
	VICIntEnClr |= 1 << 18;
	VICVectAddr5 = 0;
	VICVectCntl5 = 0;
	
	// determine the DC offset by computing the average of the samples
	short offset = mean(buf, CALIBRATE_SAMPLES);
	
	*_offset = offset;			// assign output variables atomically
	
	calibrateBuf = 0;
}

void calibrate_ISR()
{	
	// save the samples into the buffer
	calibrateBuf[calibrateIndex] = (ADGDR >> 6) & 0x3ff;
	calibrateIndex++;
	if(calibrateIndex >= CALIBRATE_SAMPLES) {
		VICIntEnClr |= (1 << 18);	// disable interrupt
		ADCR &= ~(1 << 16);			// clear burst bit (stop sampling)
		calibrateDone = 1;
	}
	VICVectAddr = 0;
}

// want to get the DC offset values for each sensor as well as the gain offsets.
// the procedure for calibration is to center the car over the track, then hit
// 'calibrate'. This function will determine offsets and scaling factors for
// each of the sensors
void sensors_calibrate()
{
	PINSEL0 |= (3 << 20) | (3 << 22) | (3 << 24);		// configure pins AD0.3, AD0.4, AD0.5
	PINSEL1 |= (3 << 12) | (3 << 14) | (3 << 16);		// configure pins AD0.0, AD0.1, AD0.2
	
	short leftMean, rightMean;
	sensor_calibrate(&sensors[SLEFT], &leftMean);
	uart0_puts("Left mean: "); uart0_puts(int2str(leftMean));
	sensor_calibrate(&sensors[SRIGHT], &rightMean);
	uart0_puts("\nRight mean: "); uart0_puts(int2str(rightMean)); uart0_puts("\n");
	
	// normalize the left and right sensors
	if(leftMean > rightMean) {
		sensors[SLEFT].scale = float2q(1.0);
		sensors[SRIGHT].scale = qdiv(short2q(leftMean), short2q(rightMean));
	} else {
		sensors[SRIGHT].scale = float2q(1.0);
		sensors[SLEFT].scale = qdiv(short2q(rightMean), short2q(leftMean));
	}
}

static volatile fixedp error = 0;
static volatile fixedp firstDiff = 0;

int is_off_track()
{
	int b = sensors[SLEFT].y < sensors[SLEFT].offTrackThresh &&
		sensors[SRIGHT].y < sensors[SRIGHT].offTrackThresh;
	return b;
}

void steer(fixedp kp)
{
	static int offTrackCount = 0;
	
	// if both sensor values are very small, then turn hard in the opposite direction
	if(is_off_track()) {
		// saturation mode. slow down, turn hard
		//motor_set(0.5, accel);
		offTrackCount++;
		// if this condition holds for 1 contiguous second, we are probably off track
		if(offTrackCount > 100) {
			motor_set(float2q(1.0), brake);
			quit = true;
		} else {
			// turn hard to the opposite side
			servo_set(int2q(-sideOfTrack));
		}
	} else {
	
		// filter the front sensor
		
		/* else if(frontCenterFiltered > 0.2 * sensors[SCENTER].centered) {
			kp = Kp;
			motor_set(1.0, accel);
		}*/
		
		// linear mode
		// error = 1/left - 1/right;
		//uart0_puts("Error: ");
		//uart0_puts(int2str(100000 * error));
		//uart0_puts("\n");
		// derivative = (error - prevError) / dt
		//fixedp error = qsub(qinv(sensors[SLEFT].y), qinv(sensors[SRIGHT].y));
		fixedp derivative = qdiv(firstDiff, dt);
		if(offTrackCount != 0) {
			// don't use the derivative since it will be bogus
			derivative = 0;
			offTrackCount = 0;
		}
		fixedp output = qadd(qmul(kp, error), qmul(Kd, derivative));
		servo_set(output);
	}
}

void process_samples()
{
	for(int i = 0; i < NUM_SENSORS; i++) {
		fixedp x = short2q(adc_buf[sensors[i].chn]);			// grab the ADC conversion result
		sensors[i].y = qfir_filter(&sensors[i].h1, x);			// low pass FIR filter
		if(sensors[i].y < int2q(1)) {
			sensors[i].y = int2q(1);
		}
	}
	frontFiltered = qfilt_filter(&frontFilt, short2q(adc_buf[sensors[SCENTER].chn]));
}

void side_of_track()
{
	if(sensors[SLEFT].y < sensors[SRIGHT].y &&
		sensors[SRIGHT].y > sensors[SRIGHT].centered &&
		sensors[SLEFT].y < sensors[SLEFT].centered)
	{
		// DEFINITELY on the left side of the track.
		sideOfTrack = LEFT;
	} else if(sensors[SLEFT].y > sensors[SRIGHT].y &&
		sensors[SRIGHT].y < sensors[SRIGHT].centered &&
		sensors[SLEFT].y > sensors[SLEFT].centered)
	{
		// DEFINITELY on right side of the track
		sideOfTrack = RIGHT;
	}
	// otherwise, just keep the same value
}

void printbuf(const short *buf, int len)
{
	for(int i = 0; i < len; i++) {
		uart0_puts(int2str(buf[i]));
		uart0_puts("\n");
	}
}

void main_loop()
{
	//short buf[2048];
	bool started = false;
	fixedp kp = Kp;
	fixedp prevError = 0;
	for(unsigned int n = 0; /* n < 2048 */; n++) {
		if(quit) {
			break;
		}
		
		// just sit for the first 100ms to let the filters stabilize
		if(!started && n > 400) {
			// grab the filtered value
			for(int i = 0; i < NUM_SENSORS; i++) {
				sensors[i].centered = sensors[i].y;
				sensors[i].offTrackThresh = qmul(float2q(0.3), sensors[i].centered);
				//uart0_puts("centered[");
				//uart0_puts(int2str(i));
				//uart0_puts("] = ");
				//uart0_puts(int2str(q2int(sensors[i].centered)));
			}
			// calculate error offset
			errorOffsetL = qsub(qinv(sensors[SCENTER].centered), qinv(sensors[SLEFT].centered));
			errorOffsetR = qsub(qinv(sensors[SCENTER].centered), qinv(sensors[SRIGHT].centered));
			motor_set(float2q(1.0), accel);
			started = true;
		}
		
		// wait for a NEW conversion to finish
		// the samplesProcessed flag is to prevent the same sample from being
		// processed twice
		while((!samplesReady || samplesProcessed) && !quit);
		FIOCLR |= 1 << 15;
		process_samples();
		side_of_track();
		
		// determine error
		fixedp newError;
#ifdef USE_3_SENSORS
		if(sideOfTrack == LEFT) {
			// use right and center sensors
			newError = qinv(sensors[SCENTER].y) - qinv(sensors[SRIGHT].y) - errorOffsetR;
		} else {
			// use the left and center sensors
			newError = -(qinv(sensors[SCENTER].y) - qinv(sensors[SLEFT].y) - errorOffsetL);
		}
#else
		newError = qinv(sensors[SLEFT].y) - qinv(sensors[SRIGHT].y);
#endif
		prevError = error;
		error = newError; //qfir_filter(&errorFilt, newError);
		firstDiff = error - prevError;
		
		
		//buf[n] = adc_buf[SENSOR_CHN[SLEFT]];
		//uart0_puts("processing sample\n");
		
#ifdef SLOW_DOWN_FOR_TURNS
		if(started) {
			if(sensors[SCENTER].y < qmul(float2q(0.8), sensors[SCENTER].centered)) {
				kp = float2q(100.0);
				motor_set(float2q(0.5), accel);
			} else if(frontFiltered > qmul(float2q(0.8), sensors[SCENTER].centered)) {
				kp = Kp;
				motor_set(float2q(1.0), accel);
			}
		}
#endif
		
		if(servo_ready()) {
			steer(kp);
		}
		FIOSET |= 1 << 15;
		samplesProcessed = true;
	}
	/*
	adc_stop();
	uart0_puts("Raw sampled data:\n");
	printbuf(buf, 2048);
	
	// filtered data
	short sampleBuf[4];
	int j = 0;
	for(unsigned int n = 0; n < 2048; n++) {
		sampleBuf[j] = buf[n];
		buf[n] = mean(sampleBuf, 4);
		j = (j + 1) % 4;
	}
	
	uart0_puts("Filtered data:\n");
	printbuf(buf, 2048);
	*/
}

int main(void)
{
	sys_init();
	uart0_init();
	spi_init();
	enableIRQ();
	
	uart0_puts("Welcome ViaCar Team 2\n");
	
	// pulse 5 times to show we're alive
	for(int i = 0; i < 5; i++) {
		led_pulse(100);
	}
	
	qfilt_init(&frontFilt, H3_SOS, H3_SOS_FRACBITS, H3_G, H3_G_FRACBITS, frontFilt_w, H3_L);
	qfir_init(&errorFilt, H4_H, H4_FRACBITS, errorFilt_w, H4_N);
	
	uart0_puts("Initializing sensors ... ");
	// initialize and calibrate the sensors
	init_sensors();
	uart0_puts("Done.\nCalibrating sensors ...\n");
	sensors_calibrate();
	uart0_puts("Done.\n");
	
	uart0_puts("Initializing motor and servo ... ");
	servo_init();
	motor_init();
	uart0_puts("Done.\n");
	
	uart0_puts("Initializing ADC and entering main loop.\n");
	// this is the main loop
	adc_init();
	
	main_loop();
	adc_stop();

	motor_set(float2q(1.0), brake);
	disableIRQ();
	uart0_puts("Exited main loop.\n");
	
	// just sit here and blink
	for(;;) {
		led_pulse(500);
	}
	
	return 0;
}

